#include <mc_path_planner/planner_base.h>

PlannerBase::PlannerBase(boost::shared_ptr<MapBase> map_handler_ptr):
    mapHandlerPtr_(map_handler_ptr),frameId_("map"){
    smoothPathPtr_ = boost::make_shared<FivePointGradientDescent>(mapHandlerPtr_);
    // 调试数据
    startPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/start_pose", 1, true);
    goalPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/goal_pose", 1, true);
    pathPub_ = nh_.advertise<nav_msgs::Path>("/global_path", 1, true);
    pathOriginPub_ = nh_.advertise<nav_msgs::Path>("/global_path_origin", 1, true);

}

PlannerBase::~PlannerBase(){

}

// 路径规划 
mc_status PlannerBase::pathPlanning(geometry_msgs::Pose start, geometry_msgs::Pose goal, 
    vector<geometry_msgs::Pose>& poses){
    mc_status status;
    ros::Time start_time = ros::Time::now();

    // 发布起点、终点
    startPosePub_.publish(msg_convert::toPoseStamped(start, frameId_));
    goalPosePub_.publish(msg_convert::toPoseStamped(goal, frameId_));

    // 起点、终点坐标转换
    Point2i start_point = mapHandlerPtr_->WorldToOpencvPixel<Point2f, Point2i>(
        msg_convert::toPoint2f(start));
    Point2i goal_point = mapHandlerPtr_->WorldToOpencvPixel<Point2f, Point2i>(
        msg_convert::toPoint2f(goal)); 
    GLOG_INFO << "[Planning | Meter] start: " << msg_convert::toString(start) 
        << ", goal: " << msg_convert::toString(goal);
    GLOG_INFO << "[Planning | Pixel] start: " << start_point.toString() << ", goal: " << goal_point.toString();

    // 路径规划
    vector<Point2i> global_points;
    vector<Point2f> global_points_2f;

    // 如果起点到终点直线上无障碍物，之间计算
    vector<Point2i> line_points;
    bool line_has_obstacle = false;
    graphis_utils::BresenhamLine(start_point, goal_point, line_points);
    for(auto& point : line_points){
        if(!mapHandlerPtr_->ValueIsValid(point)){
            line_has_obstacle = true;
            break;
        }
    }

    if(!line_has_obstacle){
        global_points = line_points;
    }
    else{
        status = pathGenerate(start_point, goal_point, global_points);
        if(status.success){
            ;   //记录错误原因
        }
    }

    // 坐标转换
    for(auto point : global_points){
        global_points_2f.emplace_back(mapHandlerPtr_->OpencvPixelToWorld<Point2i, Point2f>(point));
    }  

    // 发布原始路径
    cout << "[Planning] global_points size: " << global_points.size() << endl;
    nav_msgs::Path path_origin_msg = msg_convert::toPath(global_points_2f, frameId_);
    pathOriginPub_.publish(path_origin_msg);    


    // 路径平滑处理
    vector<Point2f> smooth_points;
    smoothPathPtr_->smoothPath(global_points_2f, smooth_points);
    nav_msgs::Path path_msg = msg_convert::toPath(smooth_points, frameId_);
    pathPub_.publish(path_msg);

    // 路径点转换
    for(auto pose_stamp : path_msg.poses){
        poses.emplace_back(pose_stamp.pose);
    }

    // 生成路径方向
    poses = genPathDirection(poses, goal);

    double time_interval = (ros::Time::now() - start_time).toSec();
    GLOG_INFO << "[PathPlanning] planning end. points size: " << poses.size()
        << ", time_interval: "  << time_interval; 
    return status;
}

string PlannerBase::toString(PlannerOption option){
    string option_str;

    return option_str;
}


// 生成路径方向
vector<geometry_msgs::Pose> PlannerBase::genPathDirection(vector<geometry_msgs::Pose>& poses, 
    geometry_msgs::Pose goal){
    vector<geometry_msgs::Pose> new_poses;
    if(poses.size() <= 2){
        new_poses = poses;
        return new_poses;
    }

    for(int i=0; i<poses.size()-1; i++){
        geometry_msgs::Pose curr_pose = poses[i];
        geometry_msgs::Pose next_pose = poses[i+1];
        double angle = math_utils::angleBetweenPoses(curr_pose, next_pose);
        curr_pose.orientation = msg_convert::toGeometryQuaternion(0.0, 0.0, angle);
        new_poses.emplace_back(curr_pose);
    }
    new_poses.emplace_back(goal);
    return new_poses;
}